#include <open62541/client_config_default.h>
#include <open62541/client_highlevel.h>
#include <open62541/plugin/log_stdout.h>
#include <open62541/client_subscriptions.h>

#include "control_client.h"

ControlClient::ControlClient() : _ns_index(4)
{
    _control_client = UA_Client_new();
    UA_ClientConfig_setDefault(UA_Client_getConfig(client));
    UA_StatusCode retVal = UA_Client_connect(client, "opc.tcp://192.168.1.1:4840");
    if (retVal != UA_STATUSCODE_GOOD)
    {
        std::cout << "连接服务器失败,请检查网络连接" << std::endl;
        UA_Client_delete(client);
        return;
    }
}

ControlClient::~ControlClient()
{
    if (_control_client != NULL)
    {
        UA_Client_delete(client);
    }
}

void ControlClient::controlPowerSupply(bool supply_power)
{
    char power_id[] = "R2";
    UA_Variant new_var;
    UA_Int32 blackout = 1;
    UA_Int32 power_feed = 2;
    UA_Variant_init(&new_var);
    const UA_NodeId node_id = UA_NODEID_STRING(_ns_index, power_id);
    if (supply_power)
    {
        UA_Variant_setScalar(&new_var, &power_feed, &UA_TYPES[UA_TYPES_INT32]);
    }
    else
    {
        UA_Variant_setScalar(&new_var, &blackout, &UA_TYPES[UA_TYPES_INT32]);
    }
    UA_StatusCode ret = UA_Client_writeValueAttribute(client, node_id, &new_var);
    if (ret == UA_STATUSCODE_GOOD)
    {
        std::cout << "写入R2: " << supply_power ? power_feed : blackout << std::endl;
    }
    else
    {
        std::cout << "写入数据失败" << std::endl;
    }
}

void ControlClient::controlEnginSpeed(float speed)
{
}

void ControlClient::enableThrottleControl(bool enable)
{
}

void ControlClient::controlLeftFrontSpeed(float speed)
{
}

void ControlClient::controlLeftRearSpeed(float speed)
{
}

void ControlClient::controlRightFrontSpeed(float speed)
{
}

void ControlClient::controlRightRearSpeed(float speed)
{
}

float ControlClient::getEnginSpeed()
{
    char node_id_str[] = "R29" UA_Variant value;
    UA_Variant_init(&value);
    UA_Float *data = (UA_Float *)value.data;
    const UA_NodeId node_id = UA_NODEID_STRING(_ns_index, node_id_str);
    if (UA_Client_readValueAttribute(client, node_id, &value) == UA_STATUSCODE_GOOD)
    {
        std::cout << "the engine speed data is: \033[32m: " << *data << std::endl;
    }
    UA_Variant_clear(&value);
    return *data;
}

float ControlClient::getEnginLeftFrontSpeed()
{
}

float ControlClient::getEnginLeftRearSpeed()
{
}

float ControlClient::getEnginRightFrontSpeed()
{
}

float ControlClient::getEnginRightRearSpeed()
{
}